﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using Microsoft.Kinect;
using Biomedica.Graphics;
using NuiDicomPlayer.Nui.Gestures;
using KinectWindows.Nui.Nui.Postures;
using Biomedica.Propability.StateMachine;
namespace SkeletonRecorder.Kinect
{
    /// <summary>
    /// Detect if a Skeleton Is Stable
    /// </summary>
    public class SkeletonStabilityDetector
    {
        private Microsoft.Kinect.Skeleton[] SkeletonBuffer;
       
        public double StabilityThreshold { get; set; }
        public Skeleton OldFrameSkeleton { set; get; }
        public bool IsStable { get; set; }
         public SkeletonStabilityDetector()
        {
            StabilityThreshold = 0.009;
           
        }
        /// <summary>
        /// Check if the Skeleton Is Stable
        /// </summary>
        /// <param name="nuSkeleton">The new Skeleton in the buffer</param>
        /// <returns>true if the skeleton Stable</returns>
         public Boolean IsSkeletonStable(Skeleton[] BufferdSkeletons)
        {
            this.SkeletonBuffer = BufferdSkeletons;
                double ma = FindDeltaAvg();
                IsStable = (ma >0.0009) & (ma < 0.001) & (ma != 0);
          
            
            return IsStable;
           
        }
       
        /// <summary>
        /// Find MA
        /// </summary>
        /// <returns></returns>
        private double FindDeltaAvg()
        {
            Vector3 maV = Vector3.Zero;
           
            Skeleton old_s=null;
            Vector3 delta = Vector3.Zero;
            foreach (Skeleton s in SkeletonBuffer)
            {

                /// Calculate the MA of center of gravity using Vecdtor3
                /// Ignore any error
                
                if(old_s!=null)
                {

                    delta = delta + (new Vector3(old_s.Position.X, old_s.Position.Y, old_s.Position.Z) - new Vector3(s.Position.X, s.Position.Y, s.Position.Z));
                }
                old_s=s;

            }
            double max = delta.X / SkeletonBuffer.Length;
            double may = delta.X / SkeletonBuffer.Length;
            double maz = delta.X / SkeletonBuffer.Length;

            return System.Math.Sqrt((max * max) + (may * may) + (maz * maz));
        }
        
    }
    /// <summary>
    /// Detect if a Joint Is Stable
    /// </summary>
    public class JointStabilityDetector : PostureDetectorBase
    {
        public JointStabilityDetector(StateContext Context)
            : base(10, Context)
        {
        }
        private Microsoft.Kinect.Skeleton[] SkeletonBuffer {set;get;}
       
        public Boolean IsStable { set; get; }
        /// <summary>
        /// Check if the Skeleton Is Stable
        /// </summary>
        /// <param name="nuSkeleton">The new Skeleton in the buffer</param>
        /// <returns>true if the Skeleton Is Stabel</returns>
        public Boolean IsSkeletonStable(Vector3 SkeletonPos)
        {

            double ma = FindDeltaAvg(SkeletonPos);
                    IsStable = ma > StabilityFactor;
                    SkeletonBuffer = null;
              
          
            return IsStable;
        }
       


        /// <summary>
        /// Find MA
        /// </summary>
        /// <returns></returns>
        private double FindDeltaAvg(Vector3 nuJoint)
        {
            Vector3 maV = Vector3.Zero;
            Skeleton old_s = null;
            Vector3 delat = Vector3.Zero;
            //foreach (Microsoft.Kinect.Skeleton s in SkeletonBuffer)
            //{

            //    /// Calculate the MA of center of gravity using Vecdtor3
            //    /// Ignore any error

            //    if (old_s != null & s!=null)
            //    {
            //        delat = delat + new Vector3(old_s.Joints[nuJoint].Position.X, old_s.Joints[nuJoint].Position.Y, old_s.Joints[nuJoint].Position.Z)
            //            - new Vector3(s.Joints[nuJoint].Position.X, s.Joints[nuJoint].Position.Y, s.Joints[nuJoint].Position.Z);
            //    }
            //    old_s = s;

            //}
            double max = delat.X / SkeletonBuffer.Length;
            double may = delat.X / SkeletonBuffer.Length;
            double maz = delat.X / SkeletonBuffer.Length;

            return System.Math.Sqrt((max * max) + (may * may) + (maz * maz));
        }

       

        public double StabilityFactor { get; set; }

        public override void TrackPosture(Vector3[] jointsPos)
        {
            throw new NotImplementedException();
        }
    }

    /// <summary>
    /// Determine if the two sholders are in the same level, Fix the sholder direction later
    /// </summary>
    public class SholderJointOrantation : PostureDetectorBase
    {

        public SholderJointOrantation(StateContext Context)
            : base( 10,Context)
        {
        }
            



        private int direction;
        public int SholderDirection { get { return direction; } }
       
        public override void TrackPosture(Vector3[] jointsPos)
        {
            if(chekSholderAreTowrdCamera(jointsPos[0],jointsPos[1]))
            {
                RaisPostureDetected("SkeletoTwordCamera");
            }
            Reset();
        }

        private bool chekSholderAreTowrdCamera(Vector3 right_shoulder, Vector3 left_shoulder)
        {
            if (!right_shoulder.HasValue || !left_shoulder.HasValue) return false;
            if(System.Math.Abs((left_shoulder.X-right_shoulder.X))>0.1) return false; 
            return true;
        }
        public void TrackPosture(Vector3 right_shoulder, Vector3 left_shoulder)
        {
            TrackPosture(new Vector3[] { right_shoulder, left_shoulder });
        }
    }

}
